clear; clc; close all

data = dlmread('C:\Users\Mittal\Documents\Work\Code\bsccs\C\codebase\CCD\Release\test-LS.txt',',', [1 1 1000 12]);
x = data(:,2:end);
y = data(:,1);
sigma2 = 1000; lambda = sqrt(2/sigma2);
tol = 0.00000001;

p = size(x,2);
n = size(x,1);

i = 0;
% range = -1:0.1:2;
% for betai = range,
%     i = i + 1;
%     j = 0;
%     for betaj = range,
%         j = j + 1;
%         logLikelihood = sum((x*[betai;betaj] - y).^2);
%         logPrior = lambda*sum(abs([betai;betaj]));
%         logPost(i,j) = logLikelihood + logPrior;
%     end
% end
% [betai, betaj] = meshgrid(range,range);
% figure; surf(betai,betaj,logPost)
% [~,ind] = min(logPost(:));
% [mini,minj] = ind2sub(size(logPost),ind);
% hold on;
% plot3(betai(1,mini),betaj(minj,1),logPost(mini,minj),'r.','markersize',30)

nIter = 1000;
beta = zeros(p,1);
logPosterior = inf;
for iIter = 1:nIter,
    logPosteriorOld = logPosterior;
    for j = 1:p,
        gradient = sum(x(:,j).*(x*beta - y));
        hessian  = sum(x(:,j).^2);
        if(beta(j) == 0)
            delta =  -(gradient + lambda)/hessian;
            if(delta < 0)
                delta = -(gradient - lambda)/hessian;
                if(delta > 0)
                    delta = 0;
                end
            end
        else
            delta = -(gradient + lambda*sign(beta(j)))/hessian;
            if(sign(beta(j))*(beta(j) + delta) < 0)
                delta = -beta(j);
            end
        end
        beta(j) = beta(j) + delta;
    end
    logLikelihood = sum((x*beta - y).^2);
    logPrior = lambda*sum(abs(beta));
    logPosterior = logLikelihood + logPrior;
    fprintf('Iter = %d\nbeta = (',iIter);
    for j = 1:p,
        fprintf('%f,',beta(j));
    end
    fprintf(')\n');
    fprintf('logPosterior = %f (%f + %f)\n\n',logPosterior,logLikelihood,logPrior);
%     hold on; plot3(beta(1),beta(2),logPosterior,'g.-','markersize',20);
    if(abs(logPosteriorOld - logPosterior)/logPosteriorOld < tol)
        fprintf('Process converged!\n');
        break;
    end
end